
tts('Program starting ...')
tic

MoveToPosition(0,90,0,50)
Claw(0)
tic
kinect_code_1
tts('large cup one')
%[arm_large(1,1), arm_large(1,2)]= moved(arm_large(1,1),arm_large(1,2),time_waste,small_arm(1,1),small_arm(1,2)); 
[x,y,z] = inv_kin(arm_large(1,1)-0.045,arm_large(1,2),arm_large(1,3)-0.01);
MoveToPosition(-x,y,z,50)
PickDropCup(2)
[x,y,z] = inv_kin(1.2*coaster_loc(1),1.2*coaster_loc(2),coaster_loc(3)+.15);
MoveToPosition(0,90,0,50)
MoveToPosition(-x,y,z,50)
PickDropCup(1)
tts('Please put condiments ')
pause
PickDropCup(2)
MoveToPosition(-90,90,0,50)
tts('Here you go ')
toc
tts('Order complete')
